#include "Timer.h"
#include "message.h"
#include "../pr.h"
#include "../hotmacCtp.h"

#define LOCAL_WAKEUP 512

module SnifferP {
  uses {
    // Control
    interface Boot;
    interface SplitControl as SerialControl;
    interface SplitControl as RadioControl;
    // Timer
    interface LocalTime<TMilli> as LocalTime;
    // Misc
    interface Receive as SnifferReceive;
    interface TimeSyncPacket<TMilli, uint32_t> as Packet;
    interface Packet as RadioPacket;
    interface Leds;
    // UART
    interface Queue<message_t*>;
    interface Pool<message_t>;
    interface AMSend as UARTSend;
  }
}
implementation {
  message_t Pkt;
  bool uartBusy = FALSE;
  uint32_t time;

  task void uartEchoTask();

  event void Boot.booted() {
    call SerialControl.start();
  }

  event void SerialControl.startDone(error_t err) {
    if (err != SUCCESS) {
      call SerialControl.start();
    } else {
      call RadioControl.start();
    }
  }

  event void RadioControl.startDone(error_t err) {
    if (err != SUCCESS) {
      call RadioControl.start();
    } else {
      time = call LocalTime.get();
      call Leds.led0On();
    }
  }

  event void RadioControl.stopDone(error_t err) { }
  event void SerialControl.stopDone(error_t err) { }

  event void UARTSend.sendDone(message_t* msg, error_t error) {
    uartBusy = FALSE;
    call Pool.put(msg);
    if (!call Queue.empty()) {
      post uartEchoTask();
    }
  }

  event message_t* SnifferReceive.receive(message_t* msg, void* payload, uint8_t len) {
    sniffer_msg_t* ptr = (sniffer_msg_t*)payload;    

    ptr->interval =  call Packet.eventTime(msg) + ptr->interval;

    call Leds.led2Toggle();
    pr("Receive a packet!\n");

    if (!call Pool.empty() && call Queue.size() < call Queue.maxSize()) {
      message_t* tmp = call Pool.get();
      if (tmp == NULL) {
        pr("The pointer from Pool is NULL!\n");
        return msg;
      }
      call Queue.enqueue(msg);
      if (!uartBusy) {
        post uartEchoTask();
      }
      return tmp;
    }

    return msg;
  }

  task void uartEchoTask() {
    if (call Queue.empty()) {
      return;
    } else if (!uartBusy) {
      message_t* tmp = call Queue.dequeue();
      uint8_t tmpLen = call RadioPacket.payloadLength(tmp);
      if (call UARTSend.send(0xffff, tmp, tmpLen) == SUCCESS) {
        uartBusy = TRUE;
      } else if (!call Queue.empty()){
        post uartEchoTask();
      }
    }
  }
}
